import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.CompassSensor;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.CompassPilot;
import lejos.robotics.navigation.Pilot;

public class Dance {

	/**
	 * Prints a string to the console,
	 * auto-scrolling and indenting if necessary
	 */
	protected void log(String s) {
		while (s.length() > 16) {
			System.out.println(s.substring(0, 16));
			s = " " + s.substring(16);
		}
		if (s != " ")
			System.out.println(s);
	}
	
	protected Pilot pilot;

	/**
	 * Resolves an angle so it is between
	 * 0 inclusive and 360 exclusive.
	 * @param angle
	 * @return The angle in a workable form.
	 */
	public static float resolveAngle(float angle) {
		while (angle<0)
			angle += 360;
		while (angle>=360)
			angle -= 360;
		return angle;
	}
	
	/**
	 * Given a first and second angle,
	 * determines the angular difference between
	 * the first angle and the second;
	 * compensates for angles out of range of 0-360;
	 * returns a positive or negative angle.
	 * @param angle1
	 * @param angle2
	 * @return The angular difference between the first and second angle.
	 */
	protected float getAngleDifference(float angle1, float angle2) {
		/**
		 * Angle1		Angle2		Angle1<Angle2	Return
		 * <0			<0			T				
		 * <0			<0			F								
		 * <0			0<Angle<360	T				
		 * <0			>360		T				
		 * 0<Angle<360	<0			F				
		 * 0<Angle<360	0<Angle<360	T				
		 * 0<Angle<360	0<Angle<360	F				
		 * 0<Angle<360	>360		F				
		 * >360			<0			F				
		 * >360			0<Angle<360	F				
		 * >360			>360		T				
		 * >360			>360		F				
		 */
		return 0;
	}
	
	/**
	 * Lets robot do a fancy dance
	 */
	protected void dance() {
		float temp, temp2;
		pilot.reset();
		java.util.Random random = new java.util.Random();
		for (int i = 0; i < 10; i++) {
			LCD.clearDisplay();
			// Get the current position and display it
			temp = Dance.resolveAngle(pilot.getAngle());
			this.log("Angle: " + temp);
//			switch (random.nextInt(5)) {
			switch ((int)(i / 2)) {
			case 0:
				this.log(i + ".0: Forward");
				temp = Dance.resolveAngle((float)(random.nextDouble() * 5 + 5));
				this.log("Trying " + temp);
				pilot.travel(temp);
				this.log("Actual " + Dance.resolveAngle(pilot.getTravelDistance()));
				break;
			case 1:
				this.log(i + ".1: Reverse");
				temp = 0 - (float)(random.nextDouble() * 5 + 5);
				this.log("Trying " + temp);
				pilot.travel(temp);
				this.log("Actual " + pilot.getTravelDistance());
				break;
			case 2:
				this.log(i + ".2: Right?");
				temp = (float)(random.nextDouble() * 45 + 30);
				temp2 = pilot.getAngle();
				this.log("Trying " + temp);
				pilot.rotate(temp);
				// Set temp to be the original angle
				temp = temp2;
				// Set temp2 to be the new angle
				temp2 = Dance.resolveAngle(pilot.getAngle());
				temp = temp2 - temp;
				this.log("Actual " + temp);
				break;
			case 3:
				this.log(i + ".3: Left?");
				temp = 0 - (float)(random.nextDouble() * 45 + 30);
				temp2 = pilot.getAngle();
				this.log("Trying " + temp);
				pilot.rotate(temp);
				// Set temp to be the original angle
				temp = temp2;
				// Set temp2 to be the new angle
				temp2 = Dance.resolveAngle(pilot.getAngle());
				temp = temp2 - temp;
				this.log("Actual " + temp);
				break;
			case 4:
				this.log(i + ".4: Party!");
				for (int j = 0; j < 8; j++)
					pilot.rotate((float)((random.nextDouble() * 20 + 20) * (random.nextInt(2) == 1 ? 1 : -1)));
				break;
			default:
				this.log(i + ".?: Unknown action!");
				break;
			}
			Button.waitForPress(5000);
		}
	}

	public static void main(String[] args) {
		RConsole.openBluetooth(1000);

		Dance traveler = new Dance();

		// Wheel Diameter: 3.6cm
		// Track Width: 13cm
		CompassPilot pilot =
			new CompassPilot(new CompassSensor(SensorPort.S3),
				3.6f, 13f, Motor.C, Motor.A);
		pilot.calibrate();

		System.out.println("Press any button...");
		Button.waitForPress();
		traveler.pilot = pilot;
		//((CompassPilot)(traveler.pilot)).setSpeed(450);
		//traveler.dance();

		// Pause for 60 seconds with output on display
		Button.waitForPress(60000);
		RConsole.close();
	}
}
